/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       INS_task.c/h
  * @brief      2025赛季哨兵欧拉角计算任务
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  V2.0.0     Nov-11-2019     RM              1. 支持bmi088，但不支持mpu6500
  *  V3.0.0     Dev-20-2024     冉文治           1. 添加卡尔曼滤波对零漂进行处理
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "INS_task.h"
#include "QuaternionEKF.h"
#include "pid.h"
#include "BMI088driver.h"
#include "bsp_imu_pwm.h"
#include "bsp_dwt.h"
#include "detect_task.h"

// PWM给定
#define IMU_temp_PWM(pwm)  imu_pwm_set(pwm)

/**
  * @brief          控制bmi088的温度
  * @author         冉文治
  * @param          temp bmi088的温度
  * @retval         none
  */
static void imu_temp_control(fp32 temp);

static const fp32 imu_temp_PID[3] = {TEMPERATURE_PID_KP, TEMPERATURE_PID_KI, TEMPERATURE_PID_KD};
static pid_type_def imu_temp_pid;

uint32_t INS_DWT_Count = 0;
static fp32 dt = 0;
static uint8_t first_temperate;

// 角速度
fp32 INS_gyro[3] = {0.0f, 0.0f, 0.0f};
// 加速度
fp32 INS_accel[3] = {0.0f, 0.0f, 0.0f};
// 四元数
fp32 INS_quat[4] = {0.0f, 0.0f, 0.0f, 0.0f};
// 欧拉角，单位rad
fp32 INS_angle[4] = {0.0f, 0.0f, 0.0f, 0.0f};

/**
  * @brief          imu任务, 初始化 bmi088, 计算欧拉角
  * @author         冉文治
  * @param          pvParameters: NULL
  * @retval         none
  */
void INS_task(void const *pvParameters)
{		
    osDelay(INS_TASK_INIT_TIME);
    // 扩展卡尔曼滤波初始化
    IMU_QuaternionEKF_Init(10, 0.001, 10000000, 0.9996, 0);
    // PID初始化
    PID_init(&imu_temp_pid, PID_POSITION, imu_temp_PID, TEMPERATURE_PID_MAX_OUT, TEMPERATURE_PID_MAX_IOUT);
		
    while(1)
    {
        // 设置时间间隔
        dt = DWT_GetDeltaT(&INS_DWT_Count);
        // 计算欧拉角
        IMU_QuaternionEKF_Update(INS_quat, INS_gyro, INS_accel, INS_angle, dt);
        // 控制温度
        imu_temp_control(BMI088.tempwhencali);
        // 任务延时
        osDelay(IMU_CONTROL_TIME_MS);
    }
}

/**
  * @brief          控制bmi088的温度
  * @author         冉文治
  * @param          temp bmi088的温度
  * @retval         none
  */
static void imu_temp_control(fp32 temp)
{
    uint16_t tempPWM;
    static uint8_t temp_constant_time = 0;
    if (first_temperate)
    {
        PID_calc(&imu_temp_pid, temp, 45.0f);
        if (imu_temp_pid.out < 0.0f)
        {
            imu_temp_pid.out = 0.0f;
        }
        tempPWM = (uint16_t)imu_temp_pid.out;
        IMU_temp_PWM(tempPWM);
    }
    else
    {
        // 在没有达到设置的温度，一直最大功率加热
        if (temp > 45.0f)
        {
            temp_constant_time++;
            if (temp_constant_time > 200)
            {
                first_temperate = 1;
                imu_temp_pid.Iout = BMI088_TEMP_PWM_MAX / 2.0f;
            }
        }

        IMU_temp_PWM(BMI088_TEMP_PWM_MAX - 1);
    }
}

/**
  * @brief          获取四元数
  * @author         冉文治
  * @param          none
  * @retval         INS_quat的指针
  */
const fp32 *get_INS_quat_point(void)
{
    return INS_quat;
}

/**
  * @brief          获取欧拉角, 0:yaw, 1:pitch, 2:roll 单位 rad
  * @author         冉文治
  * @param          none
  * @retval         INS_angle 的指针
  */
const fp32 *get_INS_angle_point(void)
{
    return INS_angle;
}

/**
  * @brief          获取角速度,0:x轴, 1:y轴, 2:roll轴 单位 rad/s
  * @author         冉文治
  * @param          none
  * @retval         INS_gyro的指针
  */
extern const fp32 *get_gyro_data_point(void)
{
    return INS_gyro;
}

/**
  * @brief          获取加速度,0:x轴, 1:y轴, 2:roll轴 单位 m/s2
  * @author         冉文治
  * @param          none
  * @retval         INS_accel的指针
  */
extern const fp32 *get_accel_data_point(void)
{
    return INS_accel;
}

/**
  * @brief          中断回调，用于检测错误
  * @author         冉文治
  * @param          GPIO_Pin
  * @retval         none
  */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if(GPIO_Pin == INT1_ACCEL_Pin)
    {
        detect_hook(BOARD_ACCEL_TOE);
    }
    else if(GPIO_Pin == INT1_GYRO_Pin)
    {
        detect_hook(BOARD_GYRO_TOE);
    }
}
